#include "data_frame.h"
#include <string>
#include "calculate_tools.h"
#include "lane_config.h"
#include "lane_const_parameter.h"

//#define DEBUG_TEST
//#define DEBUG_TEST_2

namespace rmd {

DataFrame* DataFrame::instance_ = NULL;

DataFrame::DataFrame() {}

DataFrame::~DataFrame() {}

DataFrame* DataFrame::getInstance() {
  if (instance_ == NULL) {
    instance_ = new DataFrame();
  }
  return instance_;
}

void DataFrame::Init(std::string& configFile, short width, short height) {
  LanConfig configSettings(configFile);

  error_code = true;
  auto_frame_index = 0;
  auto_calibration_flag = true;

  show_left_rho = -1;
  show_right_rho = -1;

  Update_Gap = kUpdateGap;
  Auto_Frame_Gap = kAutoFrameGap;
  Stable_Num = kStableNum;

  lIndex = 0;
  good_ldw_cout = 0;
  miss_ldw_cout = 0;

  virtual_center_lines.sflag = -1;

  // vanish point
  x_Length = kVanishPointLengthX;
  y_Length = kVanishPointLengthY;
  adapt_vanishing_point.x = kImageWidth / 2;
  adapt_vanishing_point.y = kImageHeight / 2;
  constant_vanishing_point = adapt_vanishing_point;

  refine_lane_width = kRefineLaneWidth;
  CheckGenerateLaneWidth(refine_lane_width, 1);
  InitScalarColor();

  far_line = constant_vanishing_point.y;
  end_line = std::min(far_line + kRoiHeight, kImageHeight - kRoiOffset);
  middle_start_line = far_line + kRoiOffset;
  start_line = (end_line + 2 * middle_start_line) / 3;

  up_boundary = constant_vanishing_point;
  left_boundary =
      cv::Point(std::max(kRoiOffset, (short)(up_boundary.x - kRoiWidth / 2)),
                middle_start_line + kRoiOffset);
  right_boundary = cv::Point(
      std::min(kImageWidth - kRoiOffset, up_boundary.x + kRoiWidth / 2),
      middle_start_line + kRoiOffset);

  /****************************特征提取参数************************************/
  lane_width_limit = CalcLaneWidthIndex(
      kImageHeight, end_line, kFeatureUpLaneInterval, kFeatureDownLaneInterval,
      kFeatureUdLineNum, kFeatureDownLineNo, kFeatureDLaneWidthMin,
      kFeatureDLaneWidthMax);

  CalcLaneFittingWidth(kFitInitWidth, kImageHeight, middle_start_line,
                       start_line, end_line);
  line_list = GetLineList();

  //  EKFInite();
  //  initSelectRoi();

  // debug
  //读入图像的序号
  debug_picture_index = configSettings.Read("g_picture_ID", 0);
  std::string strFilePath;
  strFilePath = configSettings.Read("FilePath", strFilePath);
  strcpy(debug_file_path, strFilePath.c_str());
  std::string strBaseDir;
  strBaseDir = configSettings.Read("BaseDir", strBaseDir);
  strcpy(debug_base_dir, strBaseDir.c_str());

  debug_index = configSettings.Read("SHOW_INDEX", 0);
  debug_show_write = configSettings.Read("SHOW_WRITER_NINDEX", 0);
}

bool DataFrame::UpdateInit(int x, int y, bool auto_check) {
  if (auto_check &&
      abs(adapt_vanishing_point.x - x) < kVanishPointLengthX / 10 &&
      abs(adapt_vanishing_point.y - y) < kVanishPointLengthY / 10) {
    return false;
  }
  constant_vanishing_point.x = x;
  constant_vanishing_point.y = y;
  adapt_vanishing_point_bf = constant_vanishing_point;
  adapt_vanishing_point = constant_vanishing_point;

  good_ldw_cout = 0;
  miss_ldw_cout = 0;

  // update ROI
  far_line = constant_vanishing_point.y;
  end_line = std::min(kImageHeight - kRoiOffset, far_line + kRoiHeight);
  middle_start_line = far_line + kRoiOffset;
  start_line = (end_line + 2 * middle_start_line) / 3;

  up_boundary = constant_vanishing_point;
  left_boundary =
      cv::Point(std::max(kRoiOffset, (short)(up_boundary.x - kRoiWidth / 2)),
                middle_start_line + kRoiOffset);
  right_boundary = cv::Point(
      std::min(kImageWidth - kRoiOffset, up_boundary.x + kRoiWidth / 2),
      middle_start_line + kRoiOffset);

  line_list = GetLineList();
  CalcLaneFittingWidth(kFitInitWidth, kImageHeight, middle_start_line,
                       start_line, end_line);

  pri_detection_results.clear();
  pri_refine_results.clear();
  InitSelectRoi();

  // init gMinLaneWidth
  float temp_lane_width = InitMinLaneWidth();
  bool scu_flag = CheckGenerateLaneWidth(temp_lane_width, 1);
  refine_lane_width = temp_lane_width;
  if (!scu_flag && auto_calibration_flag) {
    //    std::cout << "auto_flag" << auto_calibration_flag << std::endl;
    return false;
  }

  for (int i = 0; i < 4; i++) virtaul_point[i] = cv::Point(0, 0);

  return true;
}

void DataFrame::InitSelectRoi() {
  pri_lane_result.sIsUse = 0;

  short theight = end_line - far_line;  // ROI_HEIGHT
  short tBottomH = std::max(0, kImageWidth - theight);
  pri_lane_result.leftBottomPoint.x = tBottomH;
  pri_lane_result.leftBottomPoint.y = end_line;
  float resPt = CalcuateTools::GetInstance()->GetXorY(
      pri_lane_result.leftBottomPoint, adapt_vanishing_point,
      (double)middle_start_line);
  pri_lane_result.leftTopPoint.x = resPt;
  pri_lane_result.leftTopPoint.y = middle_start_line;

  tBottomH = std::min(kImageWidth, (short)(kImageWidth + theight));
  pri_lane_result.rightBottomPoint.x = tBottomH;
  pri_lane_result.rightBottomPoint.y = end_line;
  resPt = CalcuateTools::GetInstance()->GetXorY(pri_lane_result.leftBottomPoint,
                                                adapt_vanishing_point,
                                                (double)middle_start_line);
  pri_lane_result.rightTopPoint.x = resPt;
  pri_lane_result.rightTopPoint.y = middle_start_line;
}

short DataFrame::GetAdapteMaxWidth(short vanishingPointY, short startLine,
                                   short iWidth, short iHeight) {
  float t = iHeight - vanishingPointY;
  if (t <= 1e-6) return iWidth / 2;

  short ret = 2 * (startLine - vanishingPointY) * iWidth / (3 * t);

  if (ret > 0 && ret < iWidth / 2)
    return ret;
  else
    return iWidth / 2;
}

bool DataFrame::CheckGenerateLaneWidth(float& lane_width, short location) {
  bool scu_flag = true;
  switch (location) {
    case 0:

      break;
    case 2:

      break;
    default: {
      if (start_line > 5 * kImageHeight / 6) {
        if (lane_width < kImageWidth / 8) {
          lane_width = kImageWidth / 5;
          scu_flag = false;
        }
      } else if (start_line < kImageHeight / 3) {
        if (lane_width > kImageWidth / 3) {
          lane_width = kImageWidth / 8;
          scu_flag = false;
        }
      } else if (lane_width < kImageWidth / 10 ||
                 lane_width > kImageWidth / 3) {
        lane_width = kImageWidth / 6;
        scu_flag = false;
      }
    } break;
  }

  return scu_flag;
}

float DataFrame::InitMinLaneWidth() {
#ifdef DEBUG_TEST
  cv::Mat showImage;
  process_image.copyTo(showImage);
#endif

  float tretValue = 0;
  int nSize = auto_select_ld_vps.size();
  if (nSize > 0) {
    //    int adaptValue = GetAdapteMaxWidth(constant_vanishing_point.y,
    //    start_line,
    //                                       kImageWidth, kImageHeight);
    short nCount = 0;
    for (int i = 0; i < nSize; i++) {
      INITLDVP temp_ld_vp = auto_select_ld_vps[i];
      int left_offset =
          (temp_ld_vp.left_point[0].x - temp_ld_vp.left_point[1].x);
      int right_offset =
          (temp_ld_vp.right_point[0].x - temp_ld_vp.right_point[1].x);

      if (left_offset * right_offset <= 0) {
        cv::Point left_point = CalcuateTools::GetInstance()->GetXorYToPt(
            temp_ld_vp.left_point[0], temp_ld_vp.left_point[1], start_line);
        cv::Point right_point = CalcuateTools::GetInstance()->GetXorYToPt(
            temp_ld_vp.right_point[0], temp_ld_vp.right_point[1], start_line);
        float tDis = abs(left_point.x - right_point.x);
#ifdef DEBUG_TEST_2
        cv::line(showImage, left_point, right_point, scalar_color[i % 8]);
#endif
        if (/*tDis < adaptValue &&*/ tDis > kImageWidth / 20 &&
            tDis < kImageWidth / 3) {
#ifdef DEBUG_TEST
          cv::line(showImage, left_point, right_point, scalar_color[i % 8]);
#endif
          tretValue += tDis;
          nCount++;
        }
      }
    }

    if (nCount > 0) {
      tretValue = tretValue / nCount;
    }
  }
#ifdef DEBUG_TEST
  if (showImage.data != NULL) cv::imshow("VanishingPointByLSM-2", showImage);
  cv::waitKey(0);
#endif

  return tretValue;
}

std::vector<cv::Point2f> DataFrame::CalcLaneWidthIndex(
    short height, short endLine, double upLaneInterval, double downLaneInterval,
    short udLineNum, short downLineNo, double downLaneWidthMin,
    double downLaneWidthMax) {
  std::vector<cv::Point2f> laneWidthList;

  double factor_stable =
      (downLaneInterval - upLaneInterval) / (udLineNum * downLaneInterval);

  for (int lineNo = 0; lineNo < height; ++lineNo) {
    if (lineNo > endLine) {  //近场以下
      laneWidthList.push_back(laneWidthList[endLine - 1]);
    } else {
      double factor = 1 - factor_stable * (downLineNo - lineNo);
      double tLaneWidthMin = downLaneWidthMin * factor;
      double tLaneWidthMax = downLaneWidthMax * factor;
      tLaneWidthMin = (tLaneWidthMin >= 1) ? tLaneWidthMin : 1;
      tLaneWidthMax = (tLaneWidthMax >= 2) ? tLaneWidthMax : 2;
      laneWidthList.push_back(cv::Point2f(tLaneWidthMin, tLaneWidthMax));
    }
  }

  return laneWidthList;
}

void DataFrame::CalcLaneFittingWidth(const short* pInitfT_Width, short height,
                                     short middle_start_line, short start_line,
                                     short end_line) {
  this->lane_line_fitting_width.clear();
  for (int i = 0; i < height; ++i) {
    if (i < middle_start_line) {
      this->lane_line_fitting_width.push_back(pInitfT_Width[0]);
    } else if (i < start_line) {
      this->lane_line_fitting_width.push_back(pInitfT_Width[1]);
    } else if (i < end_line) {
      this->lane_line_fitting_width.push_back(pInitfT_Width[2]);
    } else {
      this->lane_line_fitting_width.push_back(pInitfT_Width[3]);
    }
  }
}

std::vector<int> DataFrame::GetLineList(int nmFieldLineNum) {
  std::vector<int> lineNumList;
  float lineNum = (nmFieldLineNum > 1) ? nmFieldLineNum : 2;
  float sumLineGap = std::max((end_line - middle_start_line), 1);
  float dValue = (2 * sumLineGap) / (lineNum * (lineNum - 1));

  int tempLineNo = end_line;
  lineNumList.push_back(end_line);
  int start = lineNum - 1;
  for (int iNum = start; iNum >= 1; --iNum) {
    double lineSpace = iNum * dValue;
    if (lineSpace > 1.0) {
      tempLineNo = tempLineNo - lineSpace;
      if (tempLineNo <= middle_start_line) {
        break;
      } else {
        lineNumList.push_back(tempLineNo);
      }
    } else {
      break;
    }
  }

  int tempStartLine;
  if (tempLineNo <= middle_start_line) {
    tempStartLine = middle_start_line;
  } else {
    tempStartLine = tempLineNo - 1;
  }

  for (int iLine = tempStartLine; iLine >= far_line; --iLine) {
    lineNumList.push_back(iLine);
  }

  return lineNumList;
}

void DataFrame::InitScalarColor() {
  scalar_color.push_back(cv::Scalar(64, 125, 255));
  scalar_color.push_back(cv::Scalar(255, 255, 0));
  scalar_color.push_back(cv::Scalar(203, 192, 255));
  scalar_color.push_back(cv::Scalar(250, 51, 153));
  scalar_color.push_back(cv::Scalar(34, 34, 178));
  scalar_color.push_back(cv::Scalar(255, 255, 255));
  scalar_color.push_back(cv::Scalar(0, 0, 255));
  scalar_color.push_back(cv::Scalar(0, 255, 255));
  scalar_color.push_back(cv::Scalar(0, 255, 0));
  scalar_color.push_back(cv::Scalar(255, 0, 0));
  scalar_color.push_back(cv::Scalar(50, 250, 50));
}
}
